package path_finding;

import java.util.*;

public class JPS implements PathFinding {
    public boolean allowDiagonal = true;
    public Heuristic heuristic = (fromX, fromY, toX, toY) -> {
        int dx = Math.abs(toX - fromX);
        int dy = Math.abs(toY - fromY);
        return Math.min(dx, dy) * 14 + Math.abs(dx - dy) * 10;
    };
    public XyTrans xyTrans;

    @Override
    public List<Integer> findPath(int[][] grid, int startX, int startY, int endX, int endY, int[][] visited) {
        Queue<Node> openSet = new PriorityQueue<>(Comparator.comparingInt(o -> o.f));
        Map<Integer, Node> allNodes = new HashMap<>();
        Set<Integer> closeSet = new HashSet<>();
        Node startNode = new Node(xyTrans.toId(startX, startY), 0,
                heuristic.calculate(startX, startY, endX, endY));
        openSet.add(startNode);
        allNodes.put(startNode.id, startNode);
        int endNodeId = xyTrans.toId(endX, endY);
        Node endNode = null;
        while (!openSet.isEmpty()) {
            Node node = openSet.poll();
            closeSet.add(node.id);
            System.out.println("选中" + node.id);
            if (node.id == endNodeId) {
                endNode = node;
                break;
            }

            int []xy = xyTrans.toXy(node.id);
            int x = xy[0];
            int y = xy[1];
            List<Integer> neighbors = new ArrayList<>(8);
            getNeighbor(node, grid, neighbors);
            for (int neighbor : neighbors) {
                System.out.println("可选----------" + neighbor);
            }
            for (int neighbor : neighbors) {
                int []neighborXy = xyTrans.toXy(neighbor);
                int nx = neighborXy[0];
                int ny = neighborXy[1];
                int dx = nx - x;
                int dy = ny - y;
                int jumpTo = jump(grid, nx, ny, dx, dy, endNodeId);
                if (jumpTo == -1 || closeSet.contains(jumpTo)) continue;

                neighborXy = xyTrans.toXy(jumpTo);
                nx = neighborXy[0];
                ny = neighborXy[1];
                visited[nx][ny] = 1;
                int disX = Math.abs(nx - x);
                int disY = Math.abs(ny - y);
                int diagonal = Math.min(disX, disY);
                int straight = Math.abs(disX - disY);
                int distance = diagonal * 14 + straight * 10;
                int g = node.g + distance;
                Node jumpNode = new Node(jumpTo, g, heuristic.calculate(neighborXy[0], neighborXy[1], endX, endY), node);
                jumpNode.dx = dx;
                jumpNode.dy = dy;
                System.out.printf("jumpTo=%d, current=%d, dir=[%d,%d]\n", jumpTo, node.id, dx, dy);
                if (!allNodes.containsKey(jumpTo)) {
                    openSet.add(jumpNode);
                    allNodes.put(jumpTo, jumpNode);
                } else {
                    Node existNode = allNodes.get(jumpTo);
                    if (jumpNode.f < existNode.f) {
                        openSet.remove(existNode);
                        openSet.add(jumpNode);
                        allNodes.put(jumpTo, jumpNode);
                    }
                }
            }
        }

        List<Integer> findWay = new LinkedList<>();
        while (endNode != null) {
            findWay.add(endNode.id);
            endNode = endNode.parent;
        }
        Collections.reverse(findWay);
        findWay = expandWay(findWay);
        return findWay;
    }

    private List<Integer> expandWay(List<Integer> findWay) {
        List<Integer> expandWay = new ArrayList<>();
        expandWay.add(findWay.get(0));
        for (int i = 0; i < findWay.size() - 1; ++ i) {
            int current = findWay.get(i);
            int next = findWay.get(i + 1);
            fillWay(current, next, expandWay);
        }
        return expandWay;
    }

    private void fillWay(int from, int to, List<Integer> way) {
        int []fromXY = xyTrans.toXy(from);
        int []toXY = xyTrans.toXy(to);
        int fromX = fromXY[0];
        int fromY = fromXY[1];
        int toX = toXY[0];
        int toY = toXY[1];
        System.out.printf("填充[%d,%d] -> [%d,%d]\n", fromX, fromY, toX, toY);

        int dx = 0;
        if (toX != fromX) {
            dx = (toX - fromX) / Math.abs(toX - fromX);
        }
        int dy = 0;
        if (fromY != toY) {
            dy = (toY - fromY) / Math.abs(toY - fromY);
        }

        while (fromX != toX || fromY != toY) {
            fromX += dx;
            fromY += dy;
            System.out.printf("填充了[%d,%d]\n", fromX, fromY);
            way.add(xyTrans.toId(fromX, fromY));
        }
    }

    private int jump(int [][]grid, int x, int y, int dx, int dy, int end) {
        int nodeId = xyTrans.toId(x, y);
        if (nodeId == end) {
            return nodeId;
        }
        if (!isWalkAble(grid, x, y)) {
            return -1;
        }

        if (nodeId == 922) {
            System.out.println("");
        }
        boolean hasForceNeighbor = hasForceNeighbor(grid, x, y, dx, dy);
        if (hasForceNeighbor) {
            System.out.println(nodeId + "有强迫邻居");
            return nodeId;
        }

        if (dx != 0 && dy != 0) {
            int jumpTo = jump(grid, x + dx, y, dx, 0, end);
            if (jumpTo != -1) {
                System.out.println(nodeId + "水平方向有跳点" + dx + 0);
                return nodeId;
            }
            jumpTo = jump(grid, x, y + dy, 0, dy, end);
            if (jumpTo != -1) {
                System.out.println(nodeId + "垂直方向有跳点" + 0 + dy);
                return nodeId;
            }
        }
        if (dx == 0 || dy == 0 || isWalkAble(grid, x + dx, y) || isWalkAble(grid, x, y + dy)) {
            return jump(grid, x + dx, y + dy, dx, dy, end);
        }
        return -1;
    }

    private boolean hasForceNeighbor(int [][]grid, int x, int y, int dx, int dy) {
        if (dx != 0 && dy != 0) {
            // 对角线移动的强迫邻居检查
            if ((!isWalkAble(grid, x - dx, y) && isWalkAble(grid, x - dx, y + dy) && isWalkAble(grid, x, y + dy)) ||
                    (!isWalkAble(grid, x, y - dy) && isWalkAble(grid, x + dx, y - dy) && isWalkAble(grid, x + dx, y))) {
                return true;
            }
        } else if (dx != 0) {
            // 水平移动的强迫邻居检查
            if (isWalkAble(grid, x + dx, y + dy)) {
                if ((!isWalkAble(grid, x, y - 1) && isWalkAble(grid, x + dx, y - 1)) ||
                        (!isWalkAble(grid, x, y + 1) && isWalkAble(grid, x + dx, y + 1))) {
                    return true;
                }
            }
        } else if (dy != 0) {
            // 垂直移动的强迫邻居检查
            if (isWalkAble(grid, x + dx, y + dy)) {
                if ((!isWalkAble(grid, x - 1, y) && isWalkAble(grid, x - 1, y + dy)) ||
                        (!isWalkAble(grid, x + 1, y) && isWalkAble(grid, x + 1, y + dy))) {
                    return true;
                }
            }
        }
        return false;
    }

    private void getNeighbor(Node node, int [][]grid, List<Integer> neighbors) {
        int nodeId = node.id;
        int []xy = xyTrans.toXy(nodeId);
        int x = xy[0];
        int y = xy[1];
        if (node.parent == null) {
            int[] cx = {-1, 0, 0, 1};
            int[] cy = {0, -1, 1, 0};
            for (int i = 0; i < cx.length; ++i) {
                int dx = cx[i];
                int dy = cy[i];
                if (isWalkAble(grid, x + dx, y + dy)) {
                    neighbors.add(xyTrans.toId(x + dx, y + dy));
                }
            }
            if (allowDiagonal) {
                int[] diagonalDx = {-1, -1, 1, 1};
                int[] diagonalDy = {-1, 1, -1, 1};
                for (int i = 0; i < diagonalDx.length; ++i) {
                    int dx = diagonalDx[i];
                    int dy = diagonalDy[i];
                    if ((isWalkAble(grid, x, y + dy) || isWalkAble(grid, x + dx, y))
                            && isWalkAble(grid, x + dx, y + dy)) {
                        neighbors.add(xyTrans.toId(x + dx, y + dy));
                    }
                }
            }
        } else {
            int dx = node.dx;
            int dy = node.dy;
            boolean R = isWalkAble(grid, x + dx, y);
            boolean F = isWalkAble(grid, x, y + dy);
            boolean L = isWalkAble(grid, x - dx, y);
            boolean B = isWalkAble(grid, x, y - dy);
            if (dx != 0 && dy != 0) {
                //前三个点
                if (R) {
                    neighbors.add(xyTrans.toId(x + dx, y));
                }
                if (F) {
                    neighbors.add(xyTrans.toId(x, y + dy));
                }
                if ((R || F) && isWalkAble(grid, x + dx, y + dy)) {
                    neighbors.add(xyTrans.toId(x + dx, y + dy));
                }
                //两个强迫邻居
                if (!L && F && isWalkAble(grid, x - dx, y + dy)) {
                    neighbors.add(xyTrans.toId(x - dx, y + dy));
                }
                if (!B && R && isWalkAble(grid, x + dx, y - dy)) {
                    neighbors.add(xyTrans.toId(x + dx, y - dy));
                }
            } else if (dy == 0) {
                if (isWalkAble(grid, x + dx, y)) {
                    neighbors.add(xyTrans.toId(x + dx, y));
                    if (!isWalkAble(grid, x, y + 1) && isWalkAble(grid, x + dx, y + 1)) {
                        neighbors.add(xyTrans.toId(x + dx, y + 1));
                    }
                    if (!isWalkAble(grid, x, y - 1) && isWalkAble(grid, x + dx, y - 1)) {
                        neighbors.add(xyTrans.toId(x + dx, y - 1));
                    }
                }
            } else {
                if (isWalkAble(grid, x, y + dy)) {
                    neighbors.add(xyTrans.toId(x, y + dy));
                    if (!isWalkAble(grid, x + 1, y) && isWalkAble(grid, x + 1, y + dy)) {
                        neighbors.add(xyTrans.toId(x + 1, y + dy));
                    }
                    if (!isWalkAble(grid, x - 1, y) && isWalkAble(grid, x - 1, y + dy)) {
                        neighbors.add(xyTrans.toId(x - 1, y + dy));
                    }
                }
            }
        }
    }

    private boolean isWalkAble(int [][]grid, int x, int y) {
        return x >= 0 && x < grid.length && y >= 0 && y < grid[0].length && grid[x][y] == 0;
    }

    @Override
    public void setAllowDiagonal(boolean allow) {
        this.allowDiagonal = allow;
    }

    @Override
    public void setHeuristic(Heuristic heuristic) {
        this.heuristic = heuristic;
    }

    @Override
    public void setXyTrans(XyTrans xyTrans) {
        this.xyTrans = xyTrans;
    }

    private static class Node {
        public int id;
        public Node parent;
        public int g;
        public int h;
        public int f;
        public int dx;
        public int dy;

        public Node(int id, int g, int h) {
            this.id = id;
            this.f = g + h;
            this.g = g;
            this.h = h;
        }

        public Node(int id, int g, int h, Node parent) {
            this.id = id;
            this.f = g + h;
            this.g = g;
            this.h = h;
            this.parent = parent;
        }
    }
}
